#include "dyn_lyapunovtimeseries.h"

LyapunovTimeSeries::LyapunovTimeSeries(mat _timeSeries, int _p, double _timeStep, double _epsilon, int _maxNumberOfPoints)
{
    setTimeSeries(_timeSeries);
    setP(_p);
    setTimeStep(_timeStep);
    setEpsilon(_epsilon);
    setMaxNumberOfPoints(_maxNumberOfPoints);
}

/*! Given a time series, we compute the Jacobian matrix of
 * at a particular point of that matrix. We closely follow
 * the algorithm set forth by Sano & Sawada (1985).
 * We find a series of points that are contained
 * within a ball of radius epsilon of the chosen orbit point.
 * For a jacobian matrix of order p, we list the points
 * that are seperated by p time steps of the points within
 * the ball.
 *
 * Using covariance matrices, we then compute the
 * evolution matrix that connects these two sets of points.
 * Because the two sets of points are good approximations
 * of the tangent space, then the matrix that connects the two
 * sets is the Jacobian matrix.
 */
mat LyapunovTimeSeries::computeJacobian(int orbitPoint, int order)
{
    // The time series is assumed to be in this configuration:
    // each row represents a timeStep, and the columns represent
    // the position of the orbit point in phase space.
    // We now construct a matrix containing as its rows the
    // orbit points within a ball of radius epsilon.
    int dimension = timeSeries.n_cols;
    mat y(1,dimension);
    mat z(1,dimension);
    unsigned int numberOfPointsFound = 0;

    for (unsigned int i=0;i<timeSeries.n_rows-order;i++)
    {
        if (i==orbitPoint){continue;}
        if (numberOfPointsFound>maxNumberOfPoints){break;}

        rowvec displacement = timeSeries.row(i)-timeSeries.row(orbitPoint);

        if (norm(displacement,2)<epsilon)
        {
            numberOfPointsFound++;
            y.resize(numberOfPointsFound,dimension);
            z.resize(numberOfPointsFound,dimension);

            y.row(numberOfPointsFound-1) = displacement;
            z.row(numberOfPointsFound-1) = timeSeries.row(i+order)-timeSeries.row(orbitPoint+order);

            if (numberOfPointsFound>50*dimension){break;}
        }

        else {continue;}
    }

    if (numberOfPointsFound==0)
    {
        cout << "Did not find any points in the sphere. Aborting" << endl;
        return eye(dimension,dimension);
    }

    if (numberOfPointsFound<dimension)
    {
        cout << "Number of points found is smaller than the dimension of the system. Degeneracy." << endl;
        return eye(dimension,dimension);
    }


    // We now have enough points in the given radius
    // We compute the matrix A using covariance matrices.
    mat V(dimension,dimension);
    mat C(dimension,dimension);

    V = (y.t()*y)/y.n_rows;
    C = (z.t()*y)/y.n_rows;

    return C*inv(V);
}

colvec LyapunovTimeSeries::computeLyapunovSpectrum(int order)
{
    // We compute the whole spectrum using the Sano & Sawada method,
    // modified to fit our needs.
    // Let us initialize the matrix of n linearly independant
    // vectors to I_d, the identity matrix (Parker, 1989, Practical
    // Numerical Algorithms for Chaotic Systems).
    unsigned int dimension = timeSeries.n_cols;
    mat independentVectors = eye(dimension,dimension);

    // The Lyapunov exponents are initially null.
    colvec lyapunov(dimension); lyapunov.fill(0.0);
    unsigned int numberOfRows = timeSeries.n_rows;

    // For the complete time series, multiply the jacobian
    // matrix by the vectors.
    for (unsigned int i=0;i<timeSeries.n_rows-order;i++)
    {
        mat jacobianMatrix = computeJacobian(i,order);

        // If the function returns precisely the identity
        // matrix, we do not count it. This is to remove
        // data points where the function has potentially failed.
        // Since returning the identity matrix is a fairly rare
        // occurence, this should work.
        if (norm(jacobianMatrix-eye(dimension,dimension),2)==0)
        {
            numberOfRows--;
            continue;

        }

        mat newVectors = jacobianMatrix*independentVectors;

        // We sum the norms of these new vectors into the Lyapunov
        // column vector.
        lyapunov += log(sqrt(sum(pow(newVectors,2.0),0))).t();

        // Now that we have collected the norm, we renormalize
        // these vectors. They will be used in the next iteration.
        vec useless1;
        mat useless2;
        svd(independentVectors, useless1, useless2, newVectors);
    }

    return lyapunov/(numberOfRows*order*timeStep);

}
